from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    # Declare parameters
    uav_namespace_arg = DeclareLaunchArgument(
        'uav_namespace',
        default_value='uav_swarms',
        description='Namespace for UAV swarms.'
    )

    vehicle_type_arg = DeclareLaunchArgument(
        'vehicle_type',
        default_value='drone',
        description='Type of vehicle.'
    )

    vehicle_num_arg = DeclareLaunchArgument(
        'vehicle_num',
        default_value="3",
        description='Number of vehicles.'
    )

    is_origin_autoinitial_arg = DeclareLaunchArgument(
        'is_origin_autoinitial',
        default_value="false",
        description='use the take off point of drone_0 as the origin point of NED coordination. '
    )

    latitude_arg = DeclareLaunchArgument(
        'latitude_num',
        default_value="25.1220303",
        description='Latitude value.'
    )

    longitude_arg = DeclareLaunchArgument(
        'longitude_num',
        default_value="121.3456422",
        description='Longitude value.'
    )

    altitude_arg = DeclareLaunchArgument(
        'altitude_num',
        default_value="531.800",
        description='Altitude value.'
    )

    # Create node
    communication_multirotor_node = Node(
        package="uav_swarms",
        executable="getlocalpos_node",
        name="getlocalpos_node",
        output="screen",
        emulate_tty=True,
        parameters=[
            {"uav_namespace": LaunchConfiguration('uav_namespace')},
            {"vehicle_num": LaunchConfiguration('vehicle_num')},
            {"vehicle_type": LaunchConfiguration('vehicle_type')},
            {"altitude_num": LaunchConfiguration('altitude_num')},  # Fixed name
            {"latitude_num": LaunchConfiguration('latitude_num')},  # Fixed name
            {"longitude_num": LaunchConfiguration('longitude_num')},  # Fixed name
            {"is_origin_autoinitial": LaunchConfiguration('is_origin_autoinitial')},  # Fixed name
        ]
    )

    # Return LaunchDescription
    return LaunchDescription([
        uav_namespace_arg,
        vehicle_type_arg,
        vehicle_num_arg,
        is_origin_autoinitial_arg,
        latitude_arg,
        longitude_arg,
        altitude_arg,
        communication_multirotor_node
    ])
